#include <joyos.h>

#include "define.h"
#include "robomove.h"
#include "led.h"
#include "bumper.h"
#include "strategy.h"
/**
 * robostraight for moving straight with assigned velocity (can be negative) 
 * no pause in this function using only the shaft encoders on the wheel
 * @requires none
 * @effects robot walks in the straight line with (hopefully) desired velocity 
 * @modifies none
 */

void roboStraight(int16_t robovel){
	//Shaft 150 / One Round Wheel
	//90*M_PI/4 cm to turn 90 degree
	//One Wheel = 3/8*9*2.54*M_PI cm
	//Motor velocity 75 per 1 round wheel
	int16_t motor_vel_left;
	int16_t motor_vel_right;
	
	int16_t encoder_left_val = encoder_read(ENCODER_PORT_LEFT);
	int16_t encoder_right_val = encoder_read(ENCODER_PORT_RIGHT);
	
	int16_t fix_encoder = (encoder_left_val - encoder_right_val) * 0.25; // good at robovel == 200
	
	roboStop();
	//int16_t fix_gyro = (gyro_get_degrees() / 360) * 5;
	
	//printf ("\nEL %d ER %d FE %d FG %d", encoder_left_val, encoder_right_val, fix_encoder, fix_gyro);
	
	if (robovel > 0) {
		motor_vel_left  = robovel - (fix_encoder);
		motor_vel_right = robovel + (fix_encoder);
	}
	else if (robovel < 0) {
		motor_vel_left  = robovel + (fix_encoder);
		motor_vel_right = robovel - (fix_encoder);
	}
	else {

		roboStop();
		motor_vel_left  = 0;
		motor_vel_right = 0;
	}

	
	
	// both motors move forward
	status = 1;
	motorSetVel(MOTOR_PORT_LEFT, motor_vel_left);
	motorSetVel(MOTOR_PORT_RIGHT, motor_vel_right);
	//printf("\nL:%d R:%d L:%d R:%d", motor_vel_left, motor_vel_right, encoder_left_val, encoder_right_val);
}

/**
 * robostop simply stops both motors from spinning
 * @requires none
 * @effects both motors brake and stop
 * @modifies none
 */
void roboStop(void){
	motor_brake(MOTOR_PORT_LEFT);
	motor_brake(MOTOR_PORT_RIGHT);
	status = 0;
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);	
}

/**
 * lineFollowFeedback provides the feedback for the robot to speed up or slow down the left
 * wheel so that the robot follows the line that is drawn under it
 * 
 * The value returned will be negative if the robot needs to move more to the left (slow down
 * the left wheel) and will be positive if the robot needs to move more to the right (speed up
 * the left wheel). The returned value of 0 means that the robot should continue the 
 * provided speed.
 *
 * The more the returned value, the more speed adjustment is required
 *
 * @requires 
 * @effects the feedback of turning slightly left or right are returned
 * @modifies none
 */
int8_t lineFollowFeedback(void){
	uint8_t left_LED_feedback = isBlack(LED_UNDER_FRONTLEFT);
	uint8_t right_LED_feedback = isBlack(LED_UNDER_FRONTRIGHT);
	uint8_t middle_LED_feedback = isBlack(LED_UNDER_CENTER);
	
	if (left_LED_feedback == 1){
		if (middle_LED_feedback == 1)
			return 1;
		else
			return 2;
	}
	else if (right_LED_feedback == 1){
		if (middle_LED_feedback == 1)
			return -1;
		else
			return -2;
	}
	else
		return 0;
}

/**
 * lineFollow runs a routine of directing the robot along the given black line.
 * if no black line is found, the robot will continue to go forward until it
 * finds one
 *
 * @requires none
 * @effects the robot moves straight along the black line
 * @modifies none
 */
void lineFollow(int16_t robovel){
	double velStep = 0.25;
	uint16_t feedback = lineFollowFeedback();
	if ((feedback > 2) || (feedback < -2)){
		roboStop();
		printf("\nLine Follow Error");
		return;
	}
	uint16_t motor_vel_right = robovel;
	uint16_t motor_vel_left = robovel * (1 + (feedback * velStep));
	
	motorSetVel(MOTOR_PORT_LEFT, motor_vel_left);
	motorSetVel(MOTOR_PORT_RIGHT, motor_vel_right);
	status = 1;
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);
	
}

/**
 * wallFollowFeedback will give a wall following feedback based on the given bumper (signified
 * by the boolean whether it is the left or the right one from robot's perspective). This is
 * assumed that the robot has already found the wall it has to follow.
 *
 * The value returned will be 0 if the robot has to move towards the wall and 1 if the
 * robot has to move away from the wall
 * @requires valid digital port of interest
			robot has found wall to follow
 * @effects returns 0 if the digital bumper is released and returns 1 if the digital
 * 			bumper is pressed down
 * @modifies none
 */
uint8_t wallFollowFeedback(bool followLeft){
	if (followLeft){
		return wallLeftBumperPressed();
	}
	else{
		return wallRightBumperPressed();
	}
}

/**
 * WallFollow directs the robot to follow the wall (given to be left or right side)
 *
 * In details, the robot steers out of the wall when the feedback shows that the sensor
 * is touching the wall. On the other hand, it steers into the wall when the feedback
 * shows that the sensor is not touching the wall.
 * 
 * @requires the given velocity is below the max speed of the robot
 * @effects the robot follows the wall of the given side
 * @modifies none
 */
void wallFollow(bool followLeft, int16_t robovel){
	double steer_left_prop = 1.4;
	double steer_right_prop = 0.6;
	uint8_t touchedWall = wallFollowFeedback(followLeft);
	uint16_t motor_vel_right = robovel;
	uint16_t motor_vel_left = robovel;
	
	switch(touchedWall){
		case(0):
			if (followLeft){
				motor_vel_right = steer_left_prop * robovel;
			}
			else{
				motor_vel_right = steer_right_prop * robovel;
			}
			break;
			
		case(1):
			if (followLeft){
				motor_vel_right = steer_right_prop * robovel;
			}
			else{
				motor_vel_right = steer_left_prop * robovel;
			}
			break;
		default:
			roboStop();
			motor_vel_right = robovel;
			printf("\nWall Follow Error");
	}
	
	if (motor_vel_left >= 250)
		motor_vel_left = 255;
	
	if (motor_vel_right >= 250)
		motor_vel_right = 255;
	
	motorSetVel(MOTOR_PORT_LEFT, motor_vel_left);
	motorSetVel(MOTOR_PORT_RIGHT, motor_vel_right);
	status = 1;
	shaftValueMonitorL = encoder_read(ENCODER_PORT_LEFT);
	shaftValueMonitorR = encoder_read(ENCODER_PORT_RIGHT);
	
}

void motorSetVel(uint8_t motor, int16_t vel){
	int16_t setvel = 0;
	if(-vel < -255){
		motor_set_vel(motor, -255);
		setvel = -255;		
	}else if(vel >255){
		motor_set_vel(motor, 255);
		setvel = 255;
	}else{
		motor_set_vel(motor, vel);	
		setvel = vel;
	}
	if(motor == MOTOR_PORT_LEFT){
		motorVelNOWL = setvel;
	}else{
		motorVelNOWR = setvel;
	}	
}

void backTilBump(void){
	bool bumped = false;
	while(!bumped){
		clearEncoder();
		roboStraight(-200);
		bumped = backBumperPressed();
	}
	pause(2000);		// let the robotcontinue to press against wall a little more
	roboStop();
}

void backToWall(uint16_t duration){
	goBack(duration);
}

void goBack (uint16_t duration) {
	// use time = 2 * duration
	/*
	for(int i =0;i<2;i++){
		motorSetVel(MOTOR_PORT_LEFT,-200);
		motorSetVel(MOTOR_PORT_RIGHT,0);
		pause((duration/2));		// let the robotcontinue to press against wall a little more
		motorSetVel(MOTOR_PORT_RIGHT,-200);
		motorSetVel(MOTOR_PORT_LEFT,0);
		pause((duration/2));
	}
	*/
	
	uint16_t i = 0;
	clearEncoder();
	
	while (i < duration) {
		roboStraight(-120);
		pause(50);
		i += 50;
		printf("\ni = %d", i);
	}
	
	roboStop();
}

